import rclpy
from rclpy.node import Node
import cv2
from sensor_msgs.msg import Image
from cv_bridge import CvBridge

class CameraDetect(Node):
    def __init__(self,source):
        super().__init__('camera_node')
        self.pub = self.create_publisher(Image,'img_raw',10)
        self.timer = self.create_timer(0.1,self.timer_callback)
        self.cap = cv2.VideoCapture(source)
        if not self.cap.isOpened():
            self.get_logger().error('SLS: Failed to open camera, please ensure your camera source is 2!')
            # rclpy.shutdown()
        self.cv_bridge=CvBridge()

    def timer_callback(self):
        ret,frame = self.cap.read()
        if ret == True:
            frame = cv2.resize(frame, (360, 180))  # 设置所需的宽度和高度
            gray_frame = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
            self.pub.publish(self.cv_bridge.cv2_to_imgmsg(gray_frame,'mono8'))

def main(args = None):
    rclpy.init(args=args)
    node = CameraDetect(0)
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()
        